#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include <chrono>
#include "turtlesim/msg/pose.hpp"
#include "chapt4_interfaces/srv/patrol.hpp"
using Patrol=chapt4_interfaces::srv::Patrol;
using namespace std::chrono_literals;
class turtlecontrol: public rclcpp::Node
{
    private:
    rclcpp::Service<Patrol>::SharedPtr patrol_server_;
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_;
    rclcpp::Subscription<turtlesim::msg::Pose>::SharedPtr subcriber_;
    double target_x_{1.0};
    double target_y_{1.0};
    double _k_{1.0};
    double max_speed_{3.0};
    public:
    explicit turtlecontrol(std::string name):Node(name)
    {
        publisher_=this->create_publisher<geometry_msgs::msg::Twist>("/turtle1/cmd_vel",10);

        subcriber_=this->create_subscription<turtlesim::msg::Pose>("/turtle1/pose",10,std::bind(&turtlecontrol::on_pose_reciever,this,std::placeholders::_1));

        patrol_server_=this->create_service<Patrol>("patrol",[&](const Patrol::Request::SharedPtr request,Patrol::Response::SharedPtr response)->void{
           if((0<request->target_x&&request->target_x<12.0f)&&(0<request->target_y&&request->target_y<12.0f))
            {
            this->target_x_=request->target_x;
            this->target_y_=request->target_y;
            response->result=Patrol::Response::SUCCESS;
            }
            else
            {
                response->result=Patrol::Response::FALL;
            }
        });
    }
    private:
    void on_pose_reciever( const turtlesim::msg::Pose::SharedPtr pose)
    {
        auto current_x=pose->x;
        auto current_y=pose->y;
        auto message = geometry_msgs::msg::Twist();
        RCLCPP_INFO(this->get_logger(),"(x=%f,y=%f)",current_x,current_y);
        auto distance=std::sqrt(
            (target_x_-current_x)*(target_x_-current_x)+
            (target_y_-current_y)*(target_y_-current_y)
        );
        auto angle = std::atan2((target_y_-current_y),(target_x_-current_x)) - pose->theta;
       
        if(distance>0.1){
            if (fabs(angle)>0.2)
            {
                message.angular.z=fabs(angle);

            }
            else{
                message.linear.x=_k_*distance;
            }
        }
        if (message.linear.x>max_speed_)
        {
            message.linear.x=max_speed_;
        }
        publisher_->publish(message);
    }

};
int main(int argc,char**argv)
{
    rclcpp::init(argc,argv);
    auto node = std::make_shared<turtlecontrol>("turtlecontrol");
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;

}